#include <buola/mat.h>
#include <buola/mat/prettyprint.h>
#include <buola/mat/crrt.h>
#include <buola/mat/cxrrt.h>
#include <buola/image/format.h>
#include <buola/image/io.h>
#include <buola/image/rasterizer/clineinterpolator.h>
#include <buola/image/drawing/basic.h>
#include <buola/image/rasterizer/cscanlinerasterizer.h>
#include <buola/image/rasterizer/cscanline.h>

using namespace buola;

class CSpace
{
public:
    CSpace()
    {
        mObstacles.emplace_back(-0.5,-1.0,0.5,-0.02);
        mObstacles.emplace_back(-0.2,0.02,0.2,0.8);
        mObstacles.emplace_back(-0.8,0.3,0.0,1.0);
    }

    mat::CVec2d RandomPoint() const
    {
        while(true)
        {
            mat::CVec2d lXRand=mat::random(2,1,random::CDefaultDistribution<double>(-1,1));

            if(!Occupied(lXRand)) return lXRand;
        }
    }

    bool Occupied(const mat::CVec2d &pP) const
    {
        for(const auto &r : mObstacles)
        {
            if(r.Contains({pP.x(),pP.y()})) return true;
        }
        
        return false;
    }

    template<typename tRenderer>
    void Draw(const tRenderer &pRenderer)
    {
        for(const auto &r : mObstacles)
            img::fill_rect(pRenderer,r.l*490.0+500.0,r.t*490.0+500.0,r.w()*490.0,r.h()*490.0,img::pixel(80,80,80));
    }

private:
    std::vector<CRect_d> mObstacles;
};

int main(int /*argc*/,char** argv)
{
    CSpace lSpace;
    start_timer();
//    auto lRRT=mat::rrt<double>(mat::CVec_d({-0.8,-0.8}),10000,0.02,lSpace);
    auto lRRT=mat::rrt<double>(mat::CVec_d({0,0}),600,0.07,lSpace);
    end_timer();
    start_timer();
//    auto lXRRT=mat::xrrt<double>(mat::CVec_d({-0.8,-0.8}),10000,0.02,lSpace);
    auto lXRRT=mat::xrrt<double>(mat::CVec_d({0,0}),600,0.07,lSpace);
    end_timer();

    img::CImage_rgb8 lImage({1000,1000},img::pixel(230,230,230));
    img::CImage_rgb8 lXImage({1000,1000},img::pixel(230,230,230));

    start_timer();
    lSpace.Draw(lImage);
    lSpace.Draw(lXImage);
    end_timer();

    start_timer();
    for(int i=0;i<lRRT.size();i++)
    {
        CPoint_i l1(lRRT.Point(i).x()*490.0+500.0,lRRT.Point(i).y()*490.0+500.0);
        CPoint_i l2(lRRT.Parent(i).x()*490.0+500.0,lRRT.Parent(i).y()*490.0+500.0);
        img::render_line_aa(lImage,l1,l2,img::pixel(50,70,130));
    }
    end_timer();

    start_timer();
    int lNTriangles=0;
    for(int i=0;i<lXRRT.size();i++)
    {
        if(lXRRT.Type(i)==mat::CXRRT<double>::TRIANGLE)
        {
            lNTriangles++;
            CPoint_i l1(lXRRT.Point(i).x()*490.0+500.0,lXRRT.Point(i).y()*490.0+500.0);
            CPoint_i l2(lXRRT.Parent(i).x()*490.0+500.0,lXRRT.Parent(i).y()*490.0+500.0);
            CPoint_i l3(lXRRT.Parent2(i).x()*490.0+500.0,lXRRT.Parent2(i).y()*490.0+500.0);
            img::CScanlineRasterizer<> lRasterizer;

            if(l1.y==l2.y&&l2.y==l3.y) continue;
            
            lRasterizer.MoveTo(l1);
            lRasterizer.LineTo(l2);
            lRasterizer.LineTo(l3);
            lRasterizer.LineTo(l1);

            img::CScanline_p8 lScanline;

            lRasterizer.RewindScanlines();
            lRasterizer.RenderScanlines(lXImage,img::pixel(50,70,130));
        }
        else if(lXRRT.Type(i)==mat::CXRRT<double>::LINE)
        {
            CPoint_i l1(lXRRT.Point(i).x()*490.0+500.0,lXRRT.Point(i).y()*490.0+500.0);
            CPoint_i l2(lXRRT.Parent(i).x()*490.0+500.0,lXRRT.Parent(i).y()*490.0+500.0);
            img::render_line_aa(lXImage,l1,l2,img::pixel(50,70,130));
        }
    }
    end_timer();
    msg_info() << lNTriangles << " triangles\n";
    
    start_timer();
    img::save(lImage,io::file("rrt.pgm"));
    img::save(lXImage,io::file("xrrt.pgm"));
    end_timer();
}